/* Gluten Library -- Vector
 *
 * A templated vector (the Euclidean kind, not the container) type. Supports
 * arbitrary dimensions, although some functionality only supports 2- and
 * 3-dimension vectors.
 *
 * $AUTHOR$    res
 * $UPDATE$    r32
 */

/* LICENSES ********************************************************************

Copyright (c) 2013- Reuben E. Smith

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.

*******************************************************************************/

#ifndef GLUTEN_VECTOR_TPP
#define GLUTEN_VECTOR_TPP

#ifdef DEBUG
   #include <cassert>
#endif

#include <cmath>
#include <cstring>

#ifndef VECTOR_NO_SIMD
    #ifdef _MSC_VER
        #include <intrin.h>
    #else
        #include <x86intrin.h>
    #endif
#endif

#include "Utility.hpp"


namespace Gluten
{
   template < unsigned int N, typename T >
   Vector<N, T>::Vector()
   {
      std::memset(_d, 0, N * sizeof(T));
   }


   template < unsigned int N, typename T >
   Vector<N, T>::Vector(const Vector<N, T>& other)
   {
      std::memcpy(_d, other._d, N * sizeof(T));
   }
   

   template < unsigned int N, typename T >
   Vector<N, T>& Vector<N, T>::operator=(const Vector<N, T>& other)
   {
      std::memcpy(_d, other._d, N * sizeof(T));
      return *this;
   }


   template < unsigned int N, typename T >
   template < typename U >
   Vector<N, T>::Vector(const U values[N])
   {
      for (unsigned int i = 0; i < N; ++i) {
         _d[i] = values[i];
      }
   }


   template < unsigned int N, typename T >
   bool Vector<N, T>::operator==(const Vector<N, T>& other) const
   {
      for (unsigned int i = 0; i < N; ++i) {
         if (!fpRelativeEqual(_d[i], other._d[i])) {
            return false;
         }
      }

      return true;
   }


   template < unsigned int N, typename T >
   bool Vector<N, T>::operator!=(const Vector<N, T>& other) const
   {
      return !operator==(other);
   }


   template < unsigned int N, typename T >
   const T& Vector<N, T>::operator()(unsigned int dim) const
   {
   #ifdef DEBUG
      assert(dim < N);
   #endif

      return _d[dim];
   }


   template < unsigned int N, typename T >
   T& Vector<N, T>::operator()(unsigned int dim)
   {
   #ifdef DEBUG
      assert(dim < N);
   #endif

      return _d[dim];
   }


   template < unsigned int N, typename T >
   T Vector<N, T>::normSquare() const
   {
      T n = 0;

      for (unsigned int i = 0; i < N; ++i) {
         n += _d[i] * _d[i];
      }

      return n;
   }


   template < unsigned int N, typename T >
   T Vector<N, T>::norm() const
   {
      return static_cast<T>(std::sqrt(static_cast<float>(normSquare())));
   }


   template < unsigned int N, typename T >
   Vector<N, T> Vector<N, T>::normal() const
   {
      Vector<N, T> v;
      T n = norm();

      if (fpRelativeEqual(n, 0.0)) {
         return v;
      }
      else for (unsigned int i = 0; i < N; ++i) {
         v._d[i] = _d[i] / n;
      }

      return v;
   }


   template < unsigned int N, typename T >
   Vector<N, T>& Vector<N, T>::normalize()
   {
      T n = norm();

      if (fpRelativeEqual(n, 0)) {
         std::memset(_d, 0, N * sizeof(T));
      }
      else for (unsigned int i = 0; i < N; ++i) {
         _d[i] /= n;
      }

      return *this;
   }


   template < unsigned int N, typename T >
   T Vector<N, T>::dot(const Vector<N, T>& other) const
   {
      T r = 0;

      for (unsigned int i = 0; i < N; ++i) {
         r += _d[i] * other._d[i];
      }

      return r;
   }


   template < unsigned int N, typename T >
   Vector<N, T> Vector<N, T>::cross(const Vector<N, T>& other) const
   {
      // This isn't neat, but didn't feel like using CRTP.

      Vector<N, T> v;

      if (N == 3) {
         v._d[0] = _d[1] * other._d[2] - _d[2] * other._d[1];
         v._d[1] = _d[2] * other._d[0] - _d[0] * other._d[2];
         v._d[2] = _d[0] * other._d[1] - _d[1] * other._d[0];
      }

      return v;
   }


   template < unsigned int N, typename T >
   Vector<N, T> Vector<N, T>::operator+() const
   {
      return Vector<N, T>(*this);
   }


   template < unsigned int N, typename T >
   Vector<N, T> Vector<N, T>::operator+(const Vector<N, T>& other) const
   {
      Vector<N, T> v;

      for (unsigned int i = 0; i < N; ++i) {
         v._d[i] = _d[i] + other._d[i];
      }

      return v;
   }


   template < unsigned int N, typename T >
   template < typename S >
   Vector<N, T> Vector<N, T>::operator+(S s) const
   {
      Vector<N, T> v;

      for (unsigned int i = 0; i < N; ++i) {
         v._d[i] = _d[i] + s;
      }

      return v;
   }


   template < unsigned int N, typename T >
   Vector<N, T>& Vector<N, T>::operator+=(const Vector<N, T>& other)
   {
      for (unsigned int i = 0; i < N; ++i) {
         _d[i] += other._d[i];
      }

      return *this;
   }


   template < unsigned int N, typename T >
   template < typename S >
   Vector<N, T>& Vector<N, T>::operator+=(S s)
   {
      for (unsigned int i = 0; i < N; ++i) {
         _d[i] += s;
      }

      return *this;
   }


   template < unsigned int N, typename T >
   Vector<N, T> Vector<N, T>::operator-() const
   {
      Vector<N, T> v;

      for (unsigned int i = 0; i < N; ++i) {
         v._d[i] = -_d[i];
      }

      return v;
   }


   template < unsigned int N, typename T >
   Vector<N, T> Vector<N, T>::operator-(const Vector<N, T>& other) const
   {
      Vector<N, T> v;

      for (unsigned int i = 0; i < N; ++i) {
         v._d[i] = _d[i] - other._d[i];
      }

      return v;
   }


   template < unsigned int N, typename T >
   template < typename S >
   Vector<N, T> Vector<N, T>::operator-(S s) const
   {
      Vector<N, T> v;

      for (unsigned int i = 0; i < N; ++i) {
         v._d[i] = _d[i] - s;
      }

      return v;
   }


   template < unsigned int N, typename T >
   Vector<N, T>& Vector<N, T>::operator-=(const Vector<N, T>& other)
   {
      for (unsigned int i = 0; i < N; ++i) {
         _d[i] -= other._d[i];
      }

      return *this;
   }


   template < unsigned int N, typename T >
   template < typename S >
   Vector<N, T>& Vector<N, T>::operator-=(S s)
   {
      for (unsigned int i = 0; i < N; ++i) {
         _d[i] -= s;
      }

      return *this;
   }


   template < unsigned int N, typename T >
   Vector<N, T> Vector<N, T>::operator*(const Vector<N, T>& other) const
   {
      Vector<N, T> v;

      for (unsigned int i = 0; i < N; ++i) {
         v._d[i] = _d[i] * other._d[i];
      }

      return v;
   }


   template < unsigned int N, typename T >
   template < typename S >
   Vector<N, T> Vector<N, T>::operator*(S s) const
   {
      Vector<N, T> v;

      for (unsigned int i = 0; i < N; ++i) {
         v._d[i] = _d[i] * s;
      }

      return v;
   }


   template < unsigned int N, typename T >
   Vector<N, T>& Vector<N, T>::operator*=(const Vector<N, T>& other)
   {
      for (unsigned int i = 0; i < N; ++i) {
         _d[i] *= other._d[i];
      }

      return *this;
   }


   template < unsigned int N, typename T >
   template < typename S >
   Vector<N, T>& Vector<N, T>::operator*=(S s)
   {
      for (unsigned int i = 0; i < N; ++i) {
         _d[i] *= s;
      }

      return *this;
   }


   template < unsigned int N, typename T >
   Vector<N, T> Vector<N, T>::operator/(const Vector<N, T>& other) const
   {
      Vector<N, T> v;

      for (unsigned int i = 0; i < N; ++i) {
         v._d[i] = _d[i] / other._d[i];
      }

      return v;
   }


   template < unsigned int N, typename T >
   template < typename S >
   Vector<N, T> Vector<N, T>::operator/(S s) const
   {
      Vector<N, T> v;

      for (unsigned int i = 0; i < N; ++i) {
         v._d[i] = _d[i] / s;
      }

      return v;
   }


   template < unsigned int N, typename T >
   Vector<N, T>& Vector<N, T>::operator/=(const Vector<N, T>& other)
   {
      for (unsigned int i = 0; i < N; ++i) {
         _d[i] /= other._d[i];
      }

      return *this;
   }


   template < unsigned int N, typename T >
   template < typename S >
   Vector<N, T>& Vector<N, T>::operator/=(S s)
   {
      for (unsigned int i = 0; i < N; ++i) {
         _d[i] /= s;
      }

      return *this;
   }


#ifndef VECTOR_NO_SIMD
   template < typename U >
   Vector<4, float>::Vector(U values[4])
   {
   #ifdef _MSC_VER
      _d = reinterpret_cast<float*>(((intptr_t)_d_raw + 15) & ~15);
   #endif

      std::memcpy(_d, reinterpret_cast<const float*>(values), 
         4 * sizeof(float));
   }


   template < typename S >
   Vector<4, float> Vector<4, float>::operator+(S s) const
   {
      Vector<4, float> r;

      _s = static_cast<float>(s);
      __m128 a = _mm_load_ps(_d);
      __m128 b = _mm_load1_ps(&_s);
      a = _mm_add_ps(a, b);
      _mm_store_ps(r._d, a);

      return r;
   }


   template < typename S >
   Vector<4, float> operator+(S s, const Vector<4, float>& v)
   {
   Vector<4, float> r;

      v._s = static_cast<float>(s);
      __m128 a = _mm_load1_ps(&v._s);
      __m128 b = _mm_load_ps(v._d);
      a = _mm_add_ps(a, b);
      _mm_store_ps(r._d, a);

      return r;
   }


   template < typename S >
   Vector<4, float>& Vector<4, float>::operator+=(S s)
   {
      _s = static_cast<float>(s);
      __m128 a = _mm_load_ps(_d);
      __m128 b = _mm_load1_ps(&_s);
      a = _mm_add_ps(a, b);
      _mm_store_ps(_d, a);

      return *this;
   }


   template < typename S >
   Vector<4, float> Vector<4, float>::operator-(S s) const
   {
      Vector<4, float> r;

      _s = static_cast<float>(s);
      __m128 a = _mm_load_ps(_d);
      __m128 b = _mm_load1_ps(&_s);
      a = _mm_sub_ps(a, b);
      _mm_store_ps(r._d, a);

      return r;
   }


   template < typename S >
   Vector<4, float> operator-(S s, const Vector<4, float>& v)
   {
      Vector<4, float> r;

      v._s = static_cast<float>(s);
      __m128 a = _mm_load1_ps(&v._s);
      __m128 b = _mm_load_ps(v._d);
      a = _mm_sub_ps(a, b);
      _mm_store_ps(r._d, a);

      return r;
   }


   template < typename S >
   Vector<4, float>& Vector<4, float>::operator-=(S s)
   {
      _s = static_cast<float>(s);
      __m128 a = _mm_load_ps(_d);
      __m128 b = _mm_load1_ps(&_s);
      a = _mm_sub_ps(a, b);
      _mm_store_ps(_d, a);

      return *this;
   }


   template < typename S >
   Vector<4, float> Vector<4, float>::operator*(S s) const
   {
      Vector<4, float> r;

      _s = static_cast<float>(s);
      __m128 a = _mm_load_ps(_d);
      __m128 b = _mm_load1_ps(&_s);
      a = _mm_mul_ps(a, b);
      _mm_store_ps(r._d, a);

      return r;
   }


   template < typename S >
   Vector<4, float> operator*(S s, const Vector<4, float>& v)
   {
      Vector<4, float> r;

      v._s = static_cast<float>(s);
      __m128 a = _mm_load1_ps(&v._s);
      __m128 b = _mm_load_ps(v._d);
      a = _mm_mul_ps(a, b);
      _mm_store_ps(r._d, a);

      return r;
   }


   template < typename S >
   Vector<4, float>& Vector<4, float>::operator*=(S s)
   {
      _s = static_cast<float>(s);
      __m128 a = _mm_load_ps(_d);
      __m128 b = _mm_load1_ps(&_s);
      a = _mm_mul_ps(a, b);
      _mm_store_ps(_d, a);

      return *this;
   }


   template < typename S >
   Vector<4, float> Vector<4, float>::operator/(S s) const
   {
      Vector<4, float> r;

      _s = static_cast<float>(s);
      __m128 a = _mm_load_ps(_d);
      __m128 b = _mm_load1_ps(&_s);
   #ifdef VECTOR_FAST_MATH
      a = _mm_mul_ps(a, _mm_rcp_ps(b));
   #else
      a = _mm_div_ps(a, b);
   #endif
      _mm_store_ps(r._d, a);

      return r;
   }


   template < typename S >
   Vector<4, float> operator/(S s, const Vector<4, float>& v)
   {
      Vector<4, float> r;

      v._s = static_cast<float>(s);
      __m128 a = _mm_load1_ps(&v._s);
      __m128 b = _mm_load_ps(v._d);
   #ifdef VECTOR_FAST_MATH
      a = _mm_mul_ps(a, _mm_rcp_ps(b));
   #else
      a = _mm_div_ps(a, b);
   #endif
      _mm_store_ps(r._d, a);

      return r;
   }


   template < typename S >
   Vector<4, float>& Vector<4, float>::operator/=(S s)
   {
      _s = static_cast<float>(s);
      __m128 a = _mm_load_ps(_d);
      __m128 b = _mm_load1_ps(&_s);
   #ifdef VECTOR_FAST_MATH
      a = _mm_mul_ps(a, _mm_rcp_ps(b));
   #else
      a = _mm_div_ps(a, b);
   #endif
      _mm_store_ps(_d, a);

      return *this;
   }
#endif
}

#endif
